
#include <plan_manage/ego_replan_fsm.h>

namespace ego_planner
{

  void EGOReplanFSM::init(ros::NodeHandle &nh)
  {
    current_wp_ = 0;
    exec_state_ = FSM_EXEC_STATE::INIT;
    have_target_ = false;
    have_odom_ = false;
    channel7_value=0;
    channel8_value=0;



    /*  fsm param  */
    nh.param("fsm/flight_type", target_type_, -1);
    nh.param("fsm/thresh_replan", replan_thresh_, -1.0);
    nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);
    nh.param("fsm/planning_horizon", planning_horizen_, -1.0);
    nh.param("fsm/planning_horizen_time", planning_horizen_time_, -1.0);
    nh.param("fsm/emergency_time_", emergency_time_, 1.0);

    nh.param("fsm/waypoint_num", waypoint_num_, -1);
    for (int i = 0; i < waypoint_num_; i++)
    {
      nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
      nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
      nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
    }

    /* initialize main modules */
    visualization_.reset(new PlanningVisualization(nh));
    planner_manager_.reset(new EGOPlannerManager);
    planner_manager_->initPlanModules(nh, visualization_);

    /* callback */
    exec_timer_ = nh.createTimer(ros::Duration(0.01), &EGOReplanFSM::execFSMCallback, this);
    safety_timer_ = nh.createTimer(ros::Duration(0.05), &EGOReplanFSM::checkCollisionCallback, this);

    // global_timer_ = nh.createTimer(ros::Duration(3.0), &EGOReplanFSM::globalplanCallback, this);
    frontier_timer_ = nh.createTimer(ros::Duration(0.5), &EGOReplanFSM::frontierTimerCallback, this);
    navigation_timer_ = nh.createTimer(ros::Duration(0.2), &EGOReplanFSM::navigationCallback, this);
    // rc_control_timer_ = nh.createTimer(ros::Duration(0.1), &EGOReplanFSM::rc_control_callback, this);

    odom_sub_ = nh.subscribe("/odom_world", 1, &EGOReplanFSM::odometryCallback, this);

    new_plan_sub_ = nh.subscribe("/zhou/neednewplan", 1, &EGOReplanFSM::newplanCallback, this);
    max_vel_sub_ = nh.subscribe("/zhou/max_vel", 1, &EGOReplanFSM::maxvelCallback, this);
    exploration_sub_ = nh.subscribe("/zhou/exploration", 1, &EGOReplanFSM::explorationCallback, this);

    mission_waypoint_sub_ = nh.subscribe("/zhou/waypoints",10,&EGOReplanFSM::mission_point_callback,this);


    marker_all_pub_ = nh.advertise<visualization_msgs::Marker>("/kdtree_box_marker", 10);
    marker_id_pub_ = nh.advertise<visualization_msgs::Marker>("/kdtree_id_marker", 1);

    bspline_pub_ = nh.advertise<ego_planner::Bspline>("/planning/bspline", 10);
    data_disp_pub_ = nh.advertise<ego_planner::DataDisp>("/planning/data_display", 100);

    frontier_point_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/zhou/frontier_point", 100);
    exploration_finished_pub_ = nh.advertise<std_msgs::Bool>("/zhou/exploration_finished", 100);

    goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    rcin_sub_ = nh.subscribe("/mavros/rc/in",10,&EGOReplanFSM::rcin_callback,this);
    px4_state_sub_ = nh.subscribe("/mavros/state",10,&EGOReplanFSM::state_callback,this);
    
    global_local_pose_sub_ = nh.subscribe("/mavros/global_position/local",10,&EGOReplanFSM::global_local_pose_callback,this);
    global_global_pose_sub_ = nh.subscribe("/mavros/global_position/global",10,&EGOReplanFSM::global_global_pose_callback,this);
    

    if (target_type_ == TARGET_TYPE::MANUAL_TARGET)
      waypoint_sub_ = nh.subscribe("/waypoint_generator/waypoints", 1, &EGOReplanFSM::waypointCallback, this);
    else if (target_type_ == TARGET_TYPE::PRESET_TARGET)
    {
      ros::Duration(1.0).sleep();
      while (ros::ok() && !have_odom_)
      {
        ros::spinOnce();
      }
        
      planGlobalTrajbyGivenWps();
    }
    else
      cout << "Wrong target_type_ value! target_type_=" << target_type_ << endl;
  }


  //gps relative
  void EGOReplanFSM::global_local_pose_callback(const nav_msgs::Odometry &msg)
  {
    // global_local_pose = msg;
    global_local_flag = true;
  }

  void EGOReplanFSM::global_global_pose_callback(const sensor_msgs::NavSatFix &msg)
  {
    gps_llh = msg;
    global_global_flag = true;
  }



  void EGOReplanFSM::mission_point_callback(const mavros_msgs::WaypointList &msg)
  {
    if(msg.waypoints.size()>0)
    {
      mavros_msgs::WaypointList mission_waypoints_temp = msg;
      mission_waypoint_flag = true;
      planGlobalTrajbyGivenMission(mission_waypoints_temp);
    }

  }

  void EGOReplanFSM::state_callback(const mavros_msgs::State::ConstPtr& msg)
  {
    px4_state = *msg;
  }

  void EGOReplanFSM::rcin_callback(const mavros_msgs::RCIn & rcvalue)
  {
    channel1_value = rcvalue.channels[0];
    channel2_value = rcvalue.channels[1];
    channel3_value = rcvalue.channels[2];
    channel4_value = rcvalue.channels[3];
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
  }

  // void EGOReplanFSM::rc_control_callback(const ros::TimerEvent &e)
  // {
  //   //here to handle rc signal
  // }

  void EGOReplanFSM::planGlobalTrajbyGivenMission(mavros_msgs::WaypointList mission_waypoints)
  {
    
    if(px4_state.mode != "OFFBOARD")
    {
      std::cout<<"====> not in offboard..."<<std::endl;
      return;
    }
    //real fly 
    // if(channel7_value<1300)
    // {
    //   std::cout<<"====> channel7 not pressed..."<<std::endl;
    //   first_to_plan_withwps = true;
    //   return;
    // }

    trigger_ = true;
    
    std::vector<Eigen::Vector3d> wps(mission_waypoints.waypoints.size());

    for (int i = 0; i < mission_waypoints.waypoints.size(); i++)
    {
      //calculate llh to local position
      wps[i](0) = mission_waypoints.waypoints[i].x_lat;
      wps[i](1) = mission_waypoints.waypoints[i].y_long;
      wps[i](2) = mission_waypoints.waypoints[i].z_alt;

      end_pt_ = wps.back();
    }
    bool success = planner_manager_->planGlobalTrajWaypoints(odom_pos_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), wps, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

    for (size_t i = 0; i < (size_t)waypoint_num_; i++)
    {
      visualization_->displayGoalPoint(wps[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i);
      ros::Duration(0.001).sleep();
    }

    if (success)
    {

      /*** display ***/
      constexpr double step_size_t = 0.1;
      int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
      std::vector<Eigen::Vector3d> gloabl_traj(i_end);
      for (int i = 0; i < i_end; i++)
      {
        gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
      }

      end_vel_.setZero();
      have_target_ = true;
      have_new_target_ = true;

      /*** FSM ***/
      if (exec_state_ == WAIT_TARGET)
      {
        changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
      }
      
      else if (exec_state_ == EXEC_TRAJ)
      {
        changeFSMExecState(REPLAN_TRAJ, "TRIG");
      }
      mission_waypoint_flag = false;

      // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
      ros::Duration(0.001).sleep();
      visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
      ros::Duration(0.001).sleep();
    }
    else
    {
      ROS_ERROR("Unable to generate global trajectory!");
    }
  }

  void EGOReplanFSM::planGlobalTrajbyGivenWps()
  {
    std::vector<Eigen::Vector3d> wps(waypoint_num_);
    for (int i = 0; i < waypoint_num_; i++)
    {
      wps[i](0) = waypoints_[i][0];
      wps[i](1) = waypoints_[i][1];
      wps[i](2) = waypoints_[i][2];

      end_pt_ = wps.back();
    }
    bool success = planner_manager_->planGlobalTrajWaypoints(odom_pos_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), wps, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

    for (size_t i = 0; i < (size_t)waypoint_num_; i++)
    {
      visualization_->displayGoalPoint(wps[i], Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, i);
      ros::Duration(0.001).sleep();
    }

    if (success)
    {

      /*** display ***/
      constexpr double step_size_t = 0.1;
      int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
      std::vector<Eigen::Vector3d> gloabl_traj(i_end);
      for (int i = 0; i < i_end; i++)
      {
        gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
      }

      end_vel_.setZero();
      have_target_ = true;
      have_new_target_ = true;

      /*** FSM ***/
      // if (exec_state_ == WAIT_TARGET)
      changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
      // else if (exec_state_ == EXEC_TRAJ)
      //   changeFSMExecState(REPLAN_TRAJ, "TRIG");

      // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
      ros::Duration(0.001).sleep();
      visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
      ros::Duration(0.001).sleep();
    }
    else
    {
      ROS_ERROR("Unable to generate global trajectory!");
    }
  }

  void EGOReplanFSM::waypointCallback(const nav_msgs::PathConstPtr &msg)
  {
    if (msg->poses[0].pose.position.z < -0.1)
      return;

    cout << "Triggered!" << endl;
    trigger_ = true;
    init_pt_ = odom_pos_;

    bool success = false;
    end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, msg->poses[0].pose.position.z;
    success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());

    visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);

    if (success)
    {

      /*** display ***/
      constexpr double step_size_t = 0.1;
      int i_end = floor(planner_manager_->global_data_.global_duration_ / step_size_t);
      vector<Eigen::Vector3d> gloabl_traj(i_end);
      for (int i = 0; i < i_end; i++)
      {
        gloabl_traj[i] = planner_manager_->global_data_.global_traj_.evaluate(i * step_size_t);
      }

      end_vel_.setZero();
      have_target_ = true;
      have_new_target_ = true;

      /*** FSM ***/
      if (exec_state_ == WAIT_TARGET)
        changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
      else if (exec_state_ == EXEC_TRAJ)
        changeFSMExecState(REPLAN_TRAJ, "TRIG");

      // visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(1, 0, 0, 1), 0.3, 0);
      visualization_->displayGlobalPathList(gloabl_traj, 0.1, 0);
    }
    else
    {
      ROS_ERROR("Unable to generate global trajectory!");
    }
  }

  void EGOReplanFSM::newplanCallback(const std_msgs::Bool &msg)
  {
    neednewplan_ = msg.data;
  }

  void EGOReplanFSM::explorationCallback(const std_msgs::Float32MultiArray &msg)
  {
    //define region to explore
    frontier_x_min = msg.data[0];
    frontier_y_min = msg.data[1];
    frontier_x_max = msg.data[2];
    frontier_y_max = msg.data[3];
    dist_height = msg.data[4];

    // resolution 1
    if(need_init_flag)
    {
      Eigen::MatrixXd temp_m = Eigen::MatrixXd::Zero(frontier_x_max-frontier_x_min,frontier_y_max-frontier_y_min);
      frontier_matrix =  temp_m;
      need_init_flag = false;
    }
    

    exploration_flag = true;
  }

  void EGOReplanFSM::maxvelCallback(const std_msgs::Float32MultiArray &msg)
  {
    // max_vel_fromrc = sqrt(msg.data[0]*msg.data[0]+msg.data[1]*msg.data[1]);
    max_vel_fromrc = abs(msg.data[0]);
    // std::cout<<"------>max_vel_fromrc: "<<max_vel_fromrc<<std::endl;
    planner_manager_->pp_.max_vel_ = max_vel_fromrc;
    planner_manager_->pp_.max_acc_ = max_vel_fromrc/2.0;
  }

  void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg)
  {
    odom_pos_(0) = msg->pose.pose.position.x;
    odom_pos_(1) = msg->pose.pose.position.y;
    odom_pos_(2) = msg->pose.pose.position.z;

    odom_vel_(0) = msg->twist.twist.linear.x;
    odom_vel_(1) = msg->twist.twist.linear.y;
    odom_vel_(2) = msg->twist.twist.linear.z;

    //odom_acc_ = estimateAcc( msg );

    odom_orient_.w() = msg->pose.pose.orientation.w;
    odom_orient_.x() = msg->pose.pose.orientation.x;
    odom_orient_.y() = msg->pose.pose.orientation.y;
    odom_orient_.z() = msg->pose.pose.orientation.z;

    have_odom_ = true;
  }

  void EGOReplanFSM::changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call)
  {

    if (new_state == exec_state_)
      continously_called_times_++;
    else
      continously_called_times_ = 1;

    static string state_str[7] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP"};
    int pre_s = int(exec_state_);
    exec_state_ = new_state;
    cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << endl;
  }

  std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> EGOReplanFSM::timesOfConsecutiveStateCalls()
  {
    return std::pair<int, FSM_EXEC_STATE>(continously_called_times_, exec_state_);
  }

  void EGOReplanFSM::printFSMExecState()
  {
    static string state_str[7] = {"INIT", "WAIT_TARGET", "GEN_NEW_TRAJ", "REPLAN_TRAJ", "EXEC_TRAJ", "EMERGENCY_STOP"};

    cout << "[FSM]: state: " + state_str[int(exec_state_)] << endl;
  }

  void EGOReplanFSM::execFSMCallback(const ros::TimerEvent &e)
  {

    static int fsm_num = 0;
    fsm_num++;
    if (fsm_num == 100)
    {
      printFSMExecState();
      if (!have_odom_)
        cout << "no odom." << endl;
      if (!trigger_)
        cout << "wait for goal." << endl;
      fsm_num = 0;
    }

    switch (exec_state_)
    {
    case INIT:
    {
      if (!have_odom_)
      {
        return;
      }
      if (!trigger_)
      {
        return;
      }
      changeFSMExecState(WAIT_TARGET, "FSM");
      break;
    }

    case WAIT_TARGET:
    {
      if (!have_target_)
      {
        
        return;
      }
        
      else
      {
        
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

    case GEN_NEW_TRAJ:
    {
      start_pt_ = odom_pos_;
      start_vel_ = odom_vel_;
      start_acc_.setZero();

      // Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
      // start_yaw_(0)         = atan2(rot_x(1), rot_x(0));
      // start_yaw_(1) = start_yaw_(2) = 0.0;

      bool flag_random_poly_init;
      if (timesOfConsecutiveStateCalls().first == 1)
        flag_random_poly_init = false;
      else
        flag_random_poly_init = true;

      bool success = callReboundReplan(true, flag_random_poly_init);
      if (success)
      {

        changeFSMExecState(EXEC_TRAJ, "FSM");
        flag_escape_emergency_ = true;
      }
      else
      {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

    case REPLAN_TRAJ:
    {

      if (planFromCurrentTraj())
      {
        changeFSMExecState(EXEC_TRAJ, "FSM");
      }
      else
      {
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }

      break;
    }

    case EXEC_TRAJ:
    {
      /* determine if need to replan */
      LocalTrajData *info = &planner_manager_->local_data_;
      ros::Time time_now = ros::Time::now();
      double t_cur = (time_now - info->start_time_).toSec();
      t_cur = min(info->duration_, t_cur);

      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);


      //by zhou to end exec
      if(neednewplan_)
      {
        have_target_ = false;
        changeFSMExecState(WAIT_TARGET, "FSM");
        return;
      }

      /* && (end_pt_ - pos).norm() < 0.5 */
      if (t_cur > info->duration_ - 1e-2)
      {
        have_target_ = false;

        changeFSMExecState(WAIT_TARGET, "FSM");
        return;
      }
      else if ((end_pt_ - pos).norm() < no_replan_thresh_)
      {
        // cout << "near end" << endl;
        return;
      }
      else if ((info->start_pos_ - pos).norm() < replan_thresh_)
      {
        // cout << "near start" << endl;
        return;
      }
      else
      {
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }
      break;
    }

    case EMERGENCY_STOP:
    {

      if (flag_escape_emergency_) // Avoiding repeated calls
      {
        callEmergencyStop(odom_pos_);
      }
      else
      {
        if (odom_vel_.norm() < 0.1)
          changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }

      flag_escape_emergency_ = false;
      break;
    }
    }

    data_disp_.header.stamp = ros::Time::now();
    data_disp_pub_.publish(data_disp_);
  }

  bool EGOReplanFSM::planFromCurrentTraj()
  {

    LocalTrajData *info = &planner_manager_->local_data_;
    ros::Time time_now = ros::Time::now();
    double t_cur = (time_now - info->start_time_).toSec();

    //cout << "info->velocity_traj_=" << info->velocity_traj_.get_control_points() << endl;

    start_pt_ = info->position_traj_.evaluateDeBoorT(t_cur);
    start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
    start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);

    bool success = callReboundReplan(false, false);

    if (!success)
    {
      success = callReboundReplan(true, false);
      //changeFSMExecState(EXEC_TRAJ, "FSM");
      if (!success)
      {
        success = callReboundReplan(true, true);
        if (!success)
        {
          return false;
        }
      }
    }

    return true;
  }

  void EGOReplanFSM::frontierTimerCallback(const ros::TimerEvent &e)
  {
    if(!exploration_flag)
    {
      return;
    }
    exploration_flag = false;
    if(!wait_target_flag)
    {
      wait_target_cnt++;
      if(wait_target_cnt>5)
      {
        wait_target_flag = true;
      }
      return;
    }
    //get the grid map
    auto map = planner_manager_->grid_map_;

    double min_xy_norm=99999;
    double max_xy_norm= 0;
    min_x_cord = 0;
    min_y_cord = 0;
    id_cnt = 0;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_pub(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<frontier_x_max-frontier_x_min;i++)
    {
      for(int j=0;j<frontier_y_max-frontier_y_min;j++)
      {
        
        if(frontier_matrix(i,j) != 0)
        {
          continue;
        }
        id_cnt++;
        double x_cord = i+frontier_x_min;
        double y_cord = j+frontier_y_min;
        double dist_to_uav = sqrt((odom_pos_(0)-x_cord)*(odom_pos_(0)-x_cord)+(odom_pos_(1)-y_cord)*(odom_pos_(1)-y_cord));
        bool hit_obstacle = false;
        double hit_meter_gain = 0.0;

        //cancel points on obstacle
        double onedegree_rad = 0.01745; // 3.14159265/180
        bool inobs_temp = false;
        if(dist_to_uav<explore_dist*2)
        {
          for(int ii=0;ii<360;ii++)
          {
            ii+=9;
            bool hit_obstacle = false;
            double hit_meter = 0.0;
            double hit_meter_gain = 0.0;
            
            double iter_temp = 0.1;
            int step_temp = floor(0.5/iter_temp);
            for(int jj=0;jj<step_temp;jj++)
            {
              Eigen::Vector3d pos_to_check2;
              pos_to_check2(0) = x_cord+jj*iter_temp*cos(ii*onedegree_rad);
              pos_to_check2(1) = y_cord+jj*iter_temp*sin(ii*onedegree_rad);
              pos_to_check2(2) = dist_height;
              if(map->getInflateOccupancy(pos_to_check2))
              {
                inobs_temp = true;
                frontier_matrix(i,j) = 1;
                break;
              }
            }
            if(inobs_temp)
            {
              break;
            }
          }
        }
        if(frontier_matrix(i,j) != 0)
        {
          continue;
        }
        //judge if the point in field
        int step_number = floor(dist_to_uav/iter_step);
        for(int k=0;k<step_number;k++)
        {
          Eigen::Vector3d pos_to_check;
          pos_to_check(0) = odom_pos_(0)+ (x_cord-odom_pos_(0))*k/(step_number*1.0);
          pos_to_check(1) = odom_pos_(1)+ (y_cord-odom_pos_(1))*k/(step_number*1.0);
          pos_to_check(2) = dist_height;
          
          if(map->getInflateOccupancy(pos_to_check))
          {
            hit_obstacle = true;
            
            break;
          }
          
        }
        if(dist_to_uav<explore_dist)
        {
          if(!hit_obstacle)
          {
            frontier_matrix(i,j) = 1;
          }
        }
        //decide where to go , principle: from x_min and y_min point to explore
        if(!hit_obstacle)
        {
          hit_meter_gain = dist_to_uav;
        }
        else{
          hit_meter_gain = dist_to_uav*10000;
        }
        if(needtochangepriciple)
        {
          if(dist_to_uav>max_xy_norm)
          {
            max_xy_norm = dist_to_uav;
            min_x_cord = x_cord;
            min_y_cord = y_cord;
          }
        }
        else
        {
          if(hit_meter_gain<min_xy_norm)
          {
            min_xy_norm = hit_meter_gain;
            min_x_cord = x_cord;
            min_y_cord = y_cord;
          }
        }
        
        pcl::PointXYZ pt;
        pt.x = x_cord;
        pt.y = y_cord;
        pt.z = 1.5;
        cloud_to_pub->points.push_back(pt);
      }
    }

    if(ros::Time::now()-last_target_time>ros::Duration(20.0))
    {
      if(abs(x_target-x_target_last)<1.0&&abs(y_target-y_target_last)<1.0&&last_target_time.toSec()>1)
      {
        if(needtochangepriciple)
        {
          needtochangepriciple = false;
        }
        else
        {
          needtochangepriciple = true;
        }
      }
      x_target_last = x_target;
      y_target_last = y_target;
      last_target_time = ros::Time::now();
    }

    x_target = min_x_cord;
    y_target = min_y_cord;

    sensor_msgs::PointCloud2 t_rosCloud;
    pcl::toROSMsg(*cloud_to_pub,t_rosCloud);
    t_rosCloud.header.frame_id = "map";
    frontier_point_pub_.publish(t_rosCloud);
    if(id_cnt==0)
    {
      std_msgs::Bool finish_msg;
      finish_msg.data = true;
      exploration_finished_pub_.publish(finish_msg);
    }
  }

  void EGOReplanFSM::navigationCallback(const ros::TimerEvent &e)
  {
    if(!exploration_flag)
    {
      return;
    }
    if(!wait_nav_flag)
    {
      nav_target_cnt++;
      if(nav_target_cnt>20)
      {
        wait_nav_flag = true;
      }
      return;
    }

    // if(id_cnt==0)
    // {
    //   std::cout<<"-------+++++++>>search complete, go to next state "<<std::endl;
    //   x_target = 0.0;
    //   y_target = 0.0;
    // }

    LocalTrajData *info1 = &planner_manager_->local_data_;
    double t_cur = (ros::Time::now() - info1->start_time_).toSec();
    double t_2_3 = info1->duration_ * 0.3;
    
    if(t_cur>t_2_3)
    {
      geometry_msgs::PoseStamped goal_msg;
      goal_msg.pose.position.x = x_target;
      goal_msg.pose.position.y = y_target;
      goal_msg.pose.position.z = dist_height;
      // std::cout<<"-------+++++++>>path execute pass 1/3.. "<<std::endl;
      goal_pub_.publish(goal_msg);
      last_nav_time = ros::Time::now();
    }
    if((ros::Time::now()-last_nav_time)>ros::Duration(15.0))
    {
      geometry_msgs::PoseStamped goal_msg;
      goal_msg.pose.position.x = x_target;
      goal_msg.pose.position.y = y_target;
      goal_msg.pose.position.z = dist_height;
      // std::cout<<"-------+++++++>>too long time no replan ,replan.. "<<std::endl;
      goal_pub_.publish(goal_msg);
      last_nav_time = ros::Time::now();
    }
  }


  void EGOReplanFSM::globalplanCallback(const ros::TimerEvent &e)
  {
    if(1)//(channel7_value>1800)
    {
      Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
      double vehicle_yaw = atan2(rot_x(1), rot_x(0));
      if(vehicle_yaw<0)
      {
        vehicle_yaw += 2*3.14159265;
      }
      auto map = planner_manager_->grid_map_;
      double exploration_dist = 10.0; 
      double iter_step = 0.5;
      int step_number = floor(exploration_dist/iter_step);
      double onedegree_rad = 0.01745; // 3.14159265/180
      Eigen::Vector3d pos_to_go;
      pos_to_go(0) = odom_pos_(0);
      pos_to_go(1) = odom_pos_(1);
      pos_to_go(2) = odom_pos_(2);
      double dist_height = 6.5;
      double max_sigma=0;
      for(int i=0;i<360;i++)
      {
        bool hit_obstacle = false;
        double hit_meter = 0.0;
        double hit_meter_gain = 0.0;
        for(int j=0;j<step_number;j++)
        {
          Eigen::Vector3d pos_to_check;
          pos_to_check(0) = odom_pos_(0)+j*iter_step*cos(i*onedegree_rad);
          pos_to_check(1) = odom_pos_(1)+j*iter_step*sin(i*onedegree_rad);
          pos_to_check(2) = dist_height;
          if(map->getInflateOccupancy(pos_to_check))
          {
            hit_obstacle = true;
            hit_meter = j*iter_step;
            hit_meter_gain = hit_meter*cos((i*onedegree_rad-vehicle_yaw)/3);
            break;
          }
        }
        if(!hit_obstacle)
        {
          hit_meter = exploration_dist;
          hit_meter_gain = hit_meter*cos((i*onedegree_rad-vehicle_yaw)/3);
        }

        if(hit_meter_gain>max_sigma)
        {
          max_sigma = hit_meter_gain;
          pos_to_go(0) = odom_pos_(0)+ hit_meter*cos(i*onedegree_rad);
          pos_to_go(1) = odom_pos_(1)+ hit_meter*sin(i*onedegree_rad);
          pos_to_go(2) = dist_height;
        }
      } 
      geometry_msgs::PoseStamped goal_msg;
      goal_msg.pose.position.x = pos_to_go(0);
      goal_msg.pose.position.y = pos_to_go(1);
      goal_msg.pose.position.z = pos_to_go(2);
      goal_pub_.publish(goal_msg);
    }
  }

  void EGOReplanFSM::checkCollisionCallback(const ros::TimerEvent &e)
  {
    LocalTrajData *info = &planner_manager_->local_data_;
    auto map = planner_manager_->grid_map_;

    if (exec_state_ == WAIT_TARGET || info->start_time_.toSec() < 1e-5)
      return;

    /* ---------- check trajectory ---------- */
    constexpr double time_step = 0.01;
    double t_cur = (ros::Time::now() - info->start_time_).toSec();
    double t_2_3 = info->duration_ * 2 / 3;
    for (double t = t_cur; t < info->duration_; t += time_step)
    {
      if (t_cur < t_2_3 && t >= t_2_3) // If t_cur < t_2_3, only the first 2/3 partition of the trajectory is considered valid and will get checked.
        break;

      if (map->getInflateOccupancy(info->position_traj_.evaluateDeBoorT(t)))
      {
        if (planFromCurrentTraj()) // Make a chance
        {
          changeFSMExecState(EXEC_TRAJ, "SAFETY");
          return;
        }
        else
        {
          if (t - t_cur < emergency_time_) // 0.8s of emergency time
          {
            ROS_WARN("Suddenly discovered obstacles. emergency stop! time=%f", t - t_cur);
            changeFSMExecState(EMERGENCY_STOP, "SAFETY");
          }
          else
          {
            //ROS_WARN("current traj in collision, replan.");
            changeFSMExecState(REPLAN_TRAJ, "SAFETY");
          }
          return;
        }
        break;
      }
    }
  }

  bool EGOReplanFSM::callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj)
  {

    getLocalTarget();

    bool plan_success =
        planner_manager_->reboundReplan(start_pt_, start_vel_, start_acc_, local_target_pt_, local_target_vel_, (have_new_target_ || flag_use_poly_init), flag_randomPolyTraj);
    have_new_target_ = false;

    cout << "final_plan_success=" << plan_success << endl;

    if (plan_success)
    {

      auto info = &planner_manager_->local_data_;

      /* publish traj */
      ego_planner::Bspline bspline;
      bspline.order = 3;
      bspline.start_time = info->start_time_;
      bspline.traj_id = info->traj_id_;

      Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
      bspline.pos_pts.reserve(pos_pts.cols());
      for (int i = 0; i < pos_pts.cols(); ++i)
      {
        geometry_msgs::Point pt;
        pt.x = pos_pts(0, i);
        pt.y = pos_pts(1, i);
        pt.z = pos_pts(2, i);
        bspline.pos_pts.push_back(pt);
      }

      Eigen::VectorXd knots = info->position_traj_.getKnot();
      bspline.knots.reserve(knots.rows());
      for (int i = 0; i < knots.rows(); ++i)
      {
        bspline.knots.push_back(knots(i));
      }

      bspline_pub_.publish(bspline);

      visualization_->displayOptimalList(info->position_traj_.get_control_points(), 0);
    }

    return plan_success;
  }
  

  bool EGOReplanFSM::callEmergencyStop(Eigen::Vector3d stop_pos)
  {

    planner_manager_->EmergencyStop(stop_pos);

    auto info = &planner_manager_->local_data_;

    /* publish traj */
    ego_planner::Bspline bspline;
    bspline.order = 3;
    bspline.start_time = info->start_time_;
    bspline.traj_id = info->traj_id_;

    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();
    bspline.pos_pts.reserve(pos_pts.cols());
    for (int i = 0; i < pos_pts.cols(); ++i)
    {
      geometry_msgs::Point pt;
      pt.x = pos_pts(0, i);
      pt.y = pos_pts(1, i);
      pt.z = pos_pts(2, i);
      bspline.pos_pts.push_back(pt);
    }

    Eigen::VectorXd knots = info->position_traj_.getKnot();
    bspline.knots.reserve(knots.rows());
    for (int i = 0; i < knots.rows(); ++i)
    {
      bspline.knots.push_back(knots(i));
    }

    bspline_pub_.publish(bspline);

    return true;
  }

  void EGOReplanFSM::getLocalTarget()
  {
    double t;
    // std::cout<<"-------->max_vel_: "<<planner_manager_->pp_.max_vel_<<std::endl;
    double t_step = planning_horizen_ / 20 / planner_manager_->pp_.max_vel_;
    double dist_min = 9999, dist_min_t = 0.0;
    for (t = planner_manager_->global_data_.last_progress_time_; t < planner_manager_->global_data_.global_duration_; t += t_step)
    {
      Eigen::Vector3d pos_t = planner_manager_->global_data_.getPosition(t);
      double dist = (pos_t - start_pt_).norm();

      if (t < planner_manager_->global_data_.last_progress_time_ + 1e-5 && dist > planning_horizen_)
      {
        // todo
        ROS_ERROR("last_progress_time_ ERROR !!!!!!!!!");
        ROS_ERROR("last_progress_time_ ERROR !!!!!!!!!");
        ROS_ERROR("last_progress_time_ ERROR !!!!!!!!!");
        ROS_ERROR("last_progress_time_ ERROR !!!!!!!!!");
        ROS_ERROR("last_progress_time_ ERROR !!!!!!!!!");
        return;
      }
      if (dist < dist_min)
      {
        dist_min = dist;
        dist_min_t = t;
      }
      if (dist >= planning_horizen_)
      {
        local_target_pt_ = pos_t;
        planner_manager_->global_data_.last_progress_time_ = dist_min_t;
        break;
      }
    }
    if (t > planner_manager_->global_data_.global_duration_) // Last global point
    {
      local_target_pt_ = end_pt_;
    }

    if ((end_pt_ - local_target_pt_).norm() < (planner_manager_->pp_.max_vel_ * planner_manager_->pp_.max_vel_) / (2 * planner_manager_->pp_.max_acc_))
    {
      // local_target_vel_ = (end_pt_ - init_pt_).normalized() * planner_manager_->pp_.max_vel_ * (( end_pt_ - local_target_pt_ ).norm() / ((planner_manager_->pp_.max_vel_*planner_manager_->pp_.max_vel_)/(2*planner_manager_->pp_.max_acc_)));
      // cout << "A" << endl;
      local_target_vel_ = Eigen::Vector3d::Zero();
    }
    else
    {
      local_target_vel_ = planner_manager_->global_data_.getVelocity(t);
      // cout << "AA" << endl;
    }
  }

} // namespace ego_planner
